using Unity.Mathematics;
using EarthUtility;
namespace EarthGeospatial
{
    public class Ellipsoid
    {
        public static Ellipsoid WGS84 = new Ellipsoid(6378137.0, 6378137.0, 6356752.3142451793);
        double3 _radii;
        double3 _radiiSquared;
        double3 _oneOverRadii;
        double3 _oneOverRadiiSquared;
        double _centerToleranceSquared;
        public Ellipsoid(double x, double y, double z) : this(new double3(x, y, z)) { }

        public Ellipsoid(double3 radii)
        {
            _radii = radii;
            _radiiSquared = new double3(radii.x * radii.x, radii.y * radii.y, radii.z * radii.z);
            _oneOverRadii = new double3(1 / radii.x, 1 / radii.y, 1 / radii.z);
            _oneOverRadiiSquared = new double3(1.0 / (radii.x * radii.x), 1.0 / (radii.y * radii.y), 1.0 / (radii.z * radii.z));
            _centerToleranceSquared = Mathd.EPSILON1;

        }

        public double3 getRadii()
        {
            return _radii;
        }

        public double3 geodeticSurfaceNormal(double3 position)
        {
            return math.normalize((position * _oneOverRadiiSquared));
        }

        public double3 geodeticSurfaceNormal(Cartographic cartographic)
        {
            double longitude = cartographic.longitude;
            double latitude = cartographic.latitude;
            double cosLatitude = math.cos(latitude);
            return math.normalize(new double3(
             cosLatitude * math.cos(longitude),
             cosLatitude * math.sin(longitude),
             math.sin(latitude)));
        }

        public double3 cartographicToCartesian(Cartographic cartographic)
        {
            double3 n = this.geodeticSurfaceNormal(cartographic);
            double3 k = this._radiiSquared * n;
            double gamma = math.sqrt(math.dot(n, k));
            k /= gamma;
            n *= cartographic.height;
            return k + n;
        }

        public Cartographic? cartesianToCartographic(double3 cartesian)
        {
            var p = this.scaleToGeodeticSurface(cartesian);

            if (!p.HasValue)
            {
                return null;
            }
            double3 n = this.geodeticSurfaceNormal(p.Value);
            double3 h = cartesian - p.Value;

            double longitude = math.atan2(n.y , n.x);
            double latitude = math.asin(n.z);
            double height = math.sign(math.dot(h, cartesian)) * math.length(h);

            return new Cartographic(longitude, latitude, height);
        }

        public double getMaximumRadius()
        {
            return math.max(this._radii.x, math.max(this._radii.y, this._radii.z));
        }

        public double getMinimumRadius()
        {
            return math.min(this._radii.x, math.min(this._radii.y, this._radii.z));
        }


        public double3? scaleToGeodeticSurface(double3 cartesian)
        {
            double positionX = cartesian.x;
            double positionY = cartesian.y;
            double positionZ = cartesian.z;

            double oneOverRadiiX = this._oneOverRadii.x;
            double oneOverRadiiY = this._oneOverRadii.y;
            double oneOverRadiiZ = this._oneOverRadii.z;

            double x2 = positionX * positionX * oneOverRadiiX * oneOverRadiiX;
            double y2 = positionY * positionY * oneOverRadiiY * oneOverRadiiY;
            double z2 = positionZ * positionZ * oneOverRadiiZ * oneOverRadiiZ;

            // Compute the squared ellipsoid norm.
            double squaredNorm = x2 + y2 + z2;
            double ratio = math.sqrt(1.0 / squaredNorm);

            // As an initial approximation, assume that the radial intersection is the
            // projection point.
            double3 intersection = cartesian * ratio;

            // If the position is near the center, the iteration will not converge.
            if (squaredNorm < this._centerToleranceSquared)
            {
                return !math.isfinite(ratio) ? (double3?)null : intersection;
            }

            double oneOverRadiiSquaredX = this._oneOverRadiiSquared.x;
            double oneOverRadiiSquaredY = this._oneOverRadiiSquared.y;
            double oneOverRadiiSquaredZ = this._oneOverRadiiSquared.z;

            // Use the gradient at the intersection point in place of the true unit
            // normal. The difference in magnitude will be absorbed in the multiplier.
            double3 gradient = new double3(
               intersection.x * oneOverRadiiSquaredX * 2.0,
               intersection.y * oneOverRadiiSquaredY * 2.0,
               intersection.z * oneOverRadiiSquaredZ * 2.0);

            // Compute the initial guess at the normal vector multiplier, lambda.
            double lambda =((1.0 - ratio) * math.length(cartesian)) / (0.5 * math.length(gradient));
            double correction = 0.0;

            double func;
            double denominator;
            double xMultiplier;
            double yMultiplier;
            double zMultiplier;
            double xMultiplier2;
            double yMultiplier2;
            double zMultiplier2;
            double xMultiplier3;
            double yMultiplier3;
            double zMultiplier3;

            do
            {
                lambda -= correction;

                xMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredX);
                yMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredY);
                zMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredZ);

                xMultiplier2 = xMultiplier * xMultiplier;
                yMultiplier2 = yMultiplier * yMultiplier;
                zMultiplier2 = zMultiplier * zMultiplier;

                xMultiplier3 = xMultiplier2 * xMultiplier;
                yMultiplier3 = yMultiplier2 * yMultiplier;
                zMultiplier3 = zMultiplier2 * zMultiplier;

                func = x2 * xMultiplier2 + y2 * yMultiplier2 + z2 * zMultiplier2 - 1.0;

                // "denominator" here refers to the use of this expression in the velocity
                // and acceleration computations in the sections to follow.
                denominator = x2 * xMultiplier3 * oneOverRadiiSquaredX +
                              y2 * yMultiplier3 * oneOverRadiiSquaredY +
                              z2 * zMultiplier3 * oneOverRadiiSquaredZ;

                double derivative = -2.0 * denominator;

                correction = func / derivative;
            } while (math.abs(func) > Mathd.EPSILON12);

            return new double3(
                positionX * xMultiplier,
                positionY * yMultiplier,
                positionZ * zMultiplier);
        }

        internal static double calcQuadtreeMaxGeometricError(Ellipsoid ellipsoid)
        {
            double mapQuality = 0.25;
            uint mapWidth = 65;
            return ellipsoid.getMaximumRadius() * mapQuality / mapWidth;
        }
    }

}